/*
 * Copyright (c) 2025 STMROS Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "stmros/stmros.hpp"
#include "stmros/plugin.hpp"
#include <class_loader/class_loader.hpp>
#include <pluginlib/class_loader.hpp>
#include <memory>
#include <thread>
#include <mutex>
#include <atomic>
#include <unordered_map>
#include <functional>
#include <vector>
#include <iostream>
#include <chrono>

namespace stmros {

// 消息ID定义
constexpr uint16_t MSG_ID_HEARTBEAT = 0;
constexpr uint16_t MSG_ID_COMMAND = 1;

StmRos::StmRos(rclcpp::Node::SharedPtr node) : 
  node_(node),
  system_id_(1),
  component_id_(1),
  connected_(false) {
  // 获取参数
  node_->declare_parameter<std::string>("fcu_url", "/dev/ttyUSB0:115200");
  node_->declare_parameter<uint8_t>("system_id", 1);
  node_->declare_parameter<uint8_t>("component_id", 1);
  
  node_->get_parameter("fcu_url", fcu_url_);
  node_->get_parameter("system_id", system_id_);
  node_->get_parameter("component_id", component_id_);
  
  // 创建发布者
  heartbeat_pub_ = node_->create_publisher<stmros_msgs::msg::StmHeartbeat>(
    "stmros/heartbeat", 10);
  
  // 创建服务
  command_srv_ = node_->create_service<stmros_msgs::srv::StmCommand>(
    "stmros/command", 
    std::bind(&StmRos::handle_command, this, std::placeholders::_1, std::placeholders::_2));
  
  // 创建定时器
  heartbeat_timer_ = node_->create_wall_timer(
    std::chrono::seconds(1), 
    std::bind(&StmRos::send_heartbeat, this));
}

StmRos::~StmRos() {
  shutdown();
}

bool StmRos::initialize() {
  // 打开连接
  conn_ = stmconn::StmConnInterface::open_url(fcu_url_);
  if (!conn_) {
    RCLCPP_ERROR(node_->get_logger(), "Failed to open connection to FCU: %s", fcu_url_.c_str());
    return false;
  }
  
  // 设置回调函数
  conn_->set_receive_callback(
    std::bind(&StmRos::message_received, this, std::placeholders::_1));
  
  conn_->set_closed_callback(
    std::bind(&StmRos::connection_closed, this));
  
  conn_->set_error_callback(
    std::bind(&StmRos::connection_error, this, std::placeholders::_1));
  
  connected_ = true;
  RCLCPP_INFO(node_->get_logger(), "Connected to FCU: %s", fcu_url_.c_str());
  
  // 初始化插件
  for (auto & plugin : plugins_) {
    plugin->initialize(conn_);
  }
  
  // 加载插件
  load_plugins();
  
  return true;
}

void StmRos::shutdown() {
  if (conn_) {
    conn_->close();
    conn_.reset();
  }
  connected_ = false;
  RCLCPP_INFO(node_->get_logger(), "STMROS shutdown");
}

void StmRos::add_plugin(plugin::PluginPtr plugin) {
  plugins_.push_back(plugin);
  
  // 如果已经连接，立即初始化插件
  if (connected_ && conn_) {
    plugin->initialize(conn_);
  }
  
  // 注册消息处理器
  auto subscriptions = plugin->get_subscriptions();
  for (const auto& sub : subscriptions) {
    uint8_t msg_id = std::get<0>(sub);
    auto handler = std::get<1>(sub);
    message_handlers_[msg_id].push_back(handler);
  }
}

bool StmRos::send_message(const stmconn::StmLinkMessage & msg) {
  if (!connected_ || !conn_) {
    return false;
  }
  
  return conn_->send_message(msg);
}

void StmRos::message_received(const stmconn::StmLinkMessage & msg) {
  // 调试打印：确认收到消息
  RCLCPP_DEBUG(node_->get_logger(), "Received message: ID=%d, System=%d, Component=%d, Size=%zu", 
               msg.message_id, msg.system_id, msg.component_id, msg.payload.size());
  
  // 首先调用插件系统的消息处理器
  auto it = message_handlers_.find(msg.message_id);
  if (it != message_handlers_.end()) {
    for (auto& handler : it->second) {
      handler(msg.payload);
    }
  }
  
  // 然后处理核心消息
  switch (msg.message_id) {
    case MSG_ID_HEARTBEAT: {
      // 调试打印：收到心跳消息
      RCLCPP_INFO(node_->get_logger(), "Received HEARTBEAT message: System=%d, Component=%d, Payload size=%zu", 
                 msg.system_id, msg.component_id, msg.payload.size());
      
      // 处理心跳消息
      if (msg.payload.size() >= 7) {
        auto heartbeat_msg = stmros_msgs::msg::StmHeartbeat();
        heartbeat_msg.system_id = msg.system_id;
        heartbeat_msg.component_id = msg.component_id;
        heartbeat_msg.type = msg.payload[0];
        heartbeat_msg.autopilot = msg.payload[1];
        heartbeat_msg.base_mode = msg.payload[2];
        
        // 对于7字节payload，custom_mode只有低字节有效
        if (msg.payload.size() >= 8) {
          heartbeat_msg.custom_mode = (static_cast<uint16_t>(msg.payload[3]) << 8) | msg.payload[4];
          heartbeat_msg.system_status = msg.payload[5];
          heartbeat_msg.stmlink_version = msg.payload[6];
        } else {
          // 7字节payload的处理
          heartbeat_msg.custom_mode = msg.payload[3];
          heartbeat_msg.system_status = msg.payload[4];
          heartbeat_msg.stmlink_version = msg.payload[5];
        }
        
        heartbeat_pub_->publish(heartbeat_msg);
        RCLCPP_DEBUG(node_->get_logger(), "Published heartbeat to ROS topic");
      } else {
        RCLCPP_WARN(node_->get_logger(), "Heartbeat message too small: %zu bytes (expected >=7)", msg.payload.size());
      }
      break;
    }
    
    default: {
      RCLCPP_DEBUG(node_->get_logger(), "Received unknown message ID: %d", msg.message_id);
      break;
    }
  }
}

void StmRos::connection_closed() {
  connected_ = false;
  RCLCPP_INFO(node_->get_logger(), "Connection to FCU closed");
}

void StmRos::connection_error(stmconn::ErrorCode code) {
  RCLCPP_ERROR(node_->get_logger(), "Connection error: %d", static_cast<int>(code));
}

void StmRos::load_plugins() {
  try {
    // 创建插件加载器
    pluginlib::ClassLoader<stmros::plugin::PluginFactory> plugin_loader("stmros", "stmros::plugin::PluginFactory");
    
    // 获取所有可用的插件
    auto plugin_classes = plugin_loader.getDeclaredClasses();
    RCLCPP_INFO(node_->get_logger(), "Found %zu plugins", plugin_classes.size());
    
    for (const auto& plugin_class : plugin_classes) {
      try {
        // 创建插件实例
        auto plugin_factory = plugin_loader.createSharedInstance(plugin_class);
        auto plugin = plugin_factory->create_plugin_instance(shared_from_this());
        
        // 添加到插件列表
        add_plugin(plugin);
        RCLCPP_INFO(node_->get_logger(), "Loaded plugin: %s", plugin_class.c_str());
      } catch (const std::exception& e) {
        RCLCPP_ERROR(node_->get_logger(), "Failed to load plugin %s: %s", plugin_class.c_str(), e.what());
      }
    }
  } catch (const std::exception& e) {
    RCLCPP_ERROR(node_->get_logger(), "Failed to initialize plugin loader: %s", e.what());
  }
}

void StmRos::send_heartbeat() {
  if (!connected_ || !conn_) {
    return;
  }
  
  // 创建心跳消息
  stmconn::StmLinkMessage msg;
  msg.system_id = system_id_;
  msg.component_id = component_id_;
  msg.message_id = MSG_ID_HEARTBEAT;
  
  // 填充心跳消息内容
  msg.payload.resize(7);
  msg.payload[0] = 1; // type
  msg.payload[1] = 0; // autopilot
  msg.payload[2] = 0; // base_mode
  msg.payload[3] = 0; // custom_mode MSB
  msg.payload[4] = 0; // custom_mode LSB
  msg.payload[5] = 0; // system_status
  msg.payload[6] = 0; // mavlink_version
  
  // 发送消息
  send_message(msg);
}

void StmRos::handle_command(const std::shared_ptr<stmros_msgs::srv::StmCommand::Request> request,
                           std::shared_ptr<stmros_msgs::srv::StmCommand::Response> response) {
  if (!connected_ || !conn_) {
    response->success = false;
    response->result_msg = "Not connected to FCU";
    return;
  }
  
  // 创建命令消息
  stmconn::StmLinkMessage msg;
  msg.system_id = system_id_;
  msg.component_id = component_id_;
  msg.message_id = MSG_ID_COMMAND;
  
  // 填充命令参数
  // 简单实现：只发送命令ID和第一个字符
  msg.payload.resize(3);
  msg.payload[0] = (request->command_id >> 8) & 0xFF;
  msg.payload[1] = request->command_id & 0xFF;
  msg.payload[2] = (request->command_param.empty()) ? 0 : request->command_param[0];
  
  // 发送消息
  if (send_message(msg)) {
    response->success = true;
    response->result_msg = "Command sent successfully";
  } else {
    response->success = false;
    response->result_msg = "Failed to send command";
  }
}

} // namespace stmros